#include "Includes.h"
///////////////////////////////////////////////////////////////////////////////////////

#define Height_Hold_Speed         15
#define Height_Keep_Lowest_Speed         -20

#define Touch_Limit_Down_Height  (SensorValue[Limit_Height_Lowest]==0)

#define hgt_section_insert 400
#define hgt_1_section_get 50
#define hgt_section_get 0


#define hgt_section_out_src_one_key 185
#define hgt_section_out_src         600

#define hgt_1_section_at_src 70

#define hgt_1_section_to_dst 0
#define hgt_2_section_to_dst 450
#define hgt_3_section_to_dst 600
#define hgt_4_section_to_dst 900
#define hgt_5_section_to_dst 1130




int GBL_hgt_Lowest=0;

///////////////////////////////////////////////////////////////////////////////////////
//UpDown
void Rise_UpDown_Func(int speed)
{
	motor[Mtr_UpDown_L1]=speed;
	motor[Mtr_UpDown_L2]=speed;
	motor[Mtr_UpDown_L3]=speed;

	motor[Mtr_UpDown_R1]=speed;
	motor[Mtr_UpDown_R2]=speed;

}

///////////////////////////////////////////////////////////////////////////////////////
void Rise_UpDown_Handle()
{
	if(      vexRT[Btn5U]  && !vexRT[Btn5D]  )
		Rise_UpDown_Func(125);
	else if( vexRT[Btn5D]  && !vexRT[Btn5U]  && ! Touch_Limit_Down_Height )
		Rise_UpDown_Func(-80);
	else if( vexRT[Btn5D]  && !vexRT[Btn5U]  &&   Touch_Limit_Down_Height )
		Rise_UpDown_Func(-35);
	else if( vexRT[Btn5D]  && vexRT[Btn5U]  && ! Touch_Limit_Down_Height) // lock
		Rise_UpDown_Func(Height_Hold_Speed);
	else if( vexRT[Btn5D]  && vexRT[Btn5U]  && Touch_Limit_Down_Height) // lock
		Rise_UpDown_Func(-25);
	else
		Rise_UpDown_Func(0);
}

///////////////////////////////////////////////////////////////////////////////////////////
void Force_UpDown_Handle()
{
	if(Special_Key)
	{
		Move_Stop();
		while(Special_Key && !vexRT[Btn8R] && !vexRT[Btn8L] && !(vexRT[Btn5D]  && vexRT[Btn5U]) )
		{
			Rise_UpDown_Func(vexRT[Ch2]);
		}
		Rise_UpDown_Func(Height_Hold_Speed);
	}
}

////////////////////////////////////////////////////////////////////////////////////////
////Encoder UpDown
int GBL_Rise_Height=0;
int GBL_Rise_Max_Speed=0;
int GBL_Rise_Min_Speed=0;
int GBL_Rise_Exit_Time=0;
TTimers GBL_Rise_Timer_x=0;
int GBL_Rise_Updown_Dir=1;
int GBL_Rise_exit_ahead=0;


task Rise_Updown_AngleSor_task()
{

	float Kp=0.4,Ki=0.0003,Kd=0;
	float Output=0;
	int Last_Err=0,Err=0;
	float Integral=0,Derivation=0;

	int Aim_Ticks=GBL_Rise_Height;
	int temp;

	/*******************************************************************************
	sem_Rise_Updown
	*************************************************************************************/
	while(1)
	{
		SemaphoreLock(sem_Rise_Updown,50);
		if(bDoesTaskOwnSemaphore(sem_Rise_Updown) )
			break;
	}
	/*******************************************************************************
	*************************************************************************************/

	ClearTimer(GBL_Rise_Timer_x);
	temp=SensorValue[Angle_Height];

	/*******************************************************************************

	*************************************************************************************/
	if(temp>=GBL_Rise_Height)///dir
		GBL_Rise_Updown_Dir=-1;
	if(temp<GBL_Rise_Height)///dir
		GBL_Rise_Updown_Dir=1;


	while(time1[GBL_Rise_Timer_x]<GBL_Rise_Exit_Time)
	{
		temp=SensorValue[Angle_Height];
		if(temp<=GBL_hgt_Lowest-500)
		{
			//writeDebugStream("XXXX\n");
			Rise_UpDown_Func(45);
			wait1Msec(1);
			continue;
		}

		////////////////////////////////////////// RightEncoder Ctrl Distance
		Err=Aim_Ticks-temp;
		if(abs(Err)<20)//blind area
			Err=0;
		else if(abs(Err)<15)
			Integral+=Err;
		Derivation=Err-Last_Err;

		Output=Kp*Err+Ki*Integral+Kd*Derivation; //calc output

		Last_Err=Err;

		if(abs(Output)>=GBL_Rise_Max_Speed) // limit output
			Output=sgn(Output)*GBL_Rise_Max_Speed;
		if(abs(Output)<GBL_Rise_Min_Speed)
			Output=sgn(Output)*GBL_Rise_Min_Speed;

		Rise_UpDown_Func(Output);

		//writeDebugStream("%d ,%.3f\n",temp,Output);

		/******************************************************************************************

		*****/
		if(GBL_Rise_exit_ahead==1)
		{
			if( GBL_Rise_Updown_Dir==1 )
			{
				if( Err==0 || temp>=Aim_Ticks )
				{
					writeDebugStream("exit:");
					break;
				}
			}
			if( GBL_Rise_Updown_Dir==-1 )
			{
				if( Err==0 || temp<=Aim_Ticks )
				{
					writeDebugStream("exit:");
					break;
				}
			}
		}
	}//while

	Rise_UpDown_Func(0);

	writeDebugStream("rise:%d, %d ,%d\n",time1[GBL_Rise_Timer_x],Aim_Ticks,temp);
	/*******************************************************************************
	*************************************************************************************/
	SemaphoreUnlock(sem_Rise_Updown);////////////////////////
}
//////////////////////////////////////////////////////////////////////////////////////////
void Rise_Updown_AngleSor_TASK_FUN(int Height,int Max_Speed,int Min_Speed,int Exit_Time,TTimers Timer_x,int exit_ahead)
{
	GBL_Rise_Height=Height;
	GBL_Rise_Max_Speed=Max_Speed;
	GBL_Rise_Min_Speed=Min_Speed;
	GBL_Rise_Exit_Time=Exit_Time;
	GBL_Rise_Timer_x=Timer_x;
	GBL_Rise_exit_ahead=exit_ahead;
	StartTask(Rise_Updown_AngleSor_task);
}
////////////////////////////////////////////////////////////////////////////////////////////

void Rise_Updown_Height_Lock_Fun(int Speed)
{
	Rise_UpDown_Func(Speed);
}
